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q (57) Abstract: The attitude of a tillable body is tracked and controlled on the basis of signals outputted from gyros and tilt sensors. 

The signals outputted from the gyros are transformed and integrated to produce estimated position information in the form of a 
Q modified quaternion in which the yaw component is constrained to a zero value. Modified quaternion information in the same form 
^ is also generated from signals outputted from the tilt sensor and is used to detect and correct an error component in the estimated 
^ position information. Gyro drift is also corrected based on the output of the tilt sensor. 
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t™ JDE ESTIMATION IN TILTABLE BODY USING 
This invention relates to tracking and control of tiltable bodies. 
5 It is known to use quaternions to represent the orientation in space of an object. 

Quaternion notation is, in general, ^v**^^*^^^ 
emp,oyed Euler data representation. Furthermore, quaternion notation is not subject to 
singularities wh,ch can occur when using Euler notation. The following U.S. patents disclose 
usmgquatennons to control, determine and/or displace orientation ofan object in space- Nos 
10 5,875,993; 5,212,480; 4,797,836; 4,742,356; and 4,737,794. 
General Bi§cussion gf£ua|ernisns 

A quaternion is a four-element, hypercomplex number first conceived by Sir William 
RowanHam 1 ltoninI843. A quaternion consists of a scalar part and a complex vector part The 
15 VeCto ~sistsofano^^ 

directionbyth.ee orthogona, complex ^ vectors: U , k . An example of a gene ral quaternion 
Q is shown below: 

Q = q0 + iql +jq2 + kq3 (£ 
Addition of quaternions is performed by adding components in like directions. 

i2=j2 = k2 = ijk = -l 

(Eq.2) 

U = -ji = k 

(Eq. 3) 

jk = -kj = i 

(Eq- 4) 

ki = -ik = j 

(Eq. 5) 
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Since the quaternion is hypercomplex, it also has a complex conjugate in which the 



direction of the vector part is reversed. An example is shown below: 



Q* =qo - iqi - jq2-kq 3 



(Eq. 6) 



The square magnitude of the quaternion can be computed by forming the product of the 



5 



quaternion with its complex conjugate as shown: 



Q 2 = QQ*=qo 2 + qi 2 + q 2 2 + q 3 2 



(Eq. 7) 



A quaternion with unit magnitude (Q 2 = 1) has special significance. Specifically, it acts 
as a two-sided rotation operator. It is worthy of note that Hamilton discovered quaternions 
through his efforts to develop a three dimensional extension of the rotational effect produced in 

1 0 the complex plane when a complex number is multiplied by a unit complex number of the form 
exp (i0). The rotational effect of exp (i0) results because multiplication of complex numbers 
requires the multiplication of their respective magnitudes and the addition of their respective 
phases. Since exp (i8) has unit magnitude, it can only affect the phase of the product. In the 
complex plane, this manifests itself as a rotation about the origin by an angle 0. In trying to 

1 5 generalize this effect to vector rotations, Hamilton originally tried three-element hypercomplex 
numbers. It was not until he realized that four elements are required to account for "phase" 
changes in three-dimensional space that he successfully produced the desired result. 

Typically, a vector rotation is accomplished using a one-sided rotation operator, R, which 
in three-dimensional space can be represented as a real, 3x3 orthogonal matrix. This 

20 transformation matrix rotates a vector, x, into a vector, x\ by multiplication on the left as shown: 



A two-sided operator must be applied using both left and right multiplication. In the case 
of the quaternion operator, the rotation is accomplished when a given quaternion (X) with a null 



x' = Rx 



where x e R 3x] and Re R 



,3x3 



(Eq. 8) 
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scaur pa* (,, . vector) „ pre . md by fc ^ ^ ^ ^ ^ 

shown: 

X' = QXQ* 

(Eq. 9) 

The taring vector, X-, is roB1 ed abou, a genera, axis by , specific ar, 8 ,e, both „ f whic 

and ,he a„ gl e of rotation ,s denoted by an mgle , 9, ta the ^ quatemio „ cm ^ 

written as: 

qo-eos(6/2) and a-nainfflm 

q nsm<9/2) where q = (q,, qi ,qj) 



10 



(Eq. 10) 

These components satisfy the normalization condition: 

i-*' www 

The quaternion component thus defined are also caned the Euler Pammeters These 
parameters contain tf of me infotmation ncceasary ,„ derive the axis and angfc of rotata The 

me onesided rotation ntatrtx, R, responding to the eigenvalue, X - +1 . This ^ ^ 
— isofrotartonmnsthecommonhahothmeoHgina,^ rotate, hmes and therefore must 
be unchanged hy the rotation operator. No,, ma, m. so-caned eigenaxis rotation is a si„ 8 ,e 
m,a,,o„ abon, a genem, axis as compared to ,hc Eu,er angle ro,a,ion which .ceomplishes the 

and x axes, respectively. 

The quaternion comp.ex urn, vectors ,,, j, k) arc ,e,a,ed ,o fa Pauli spin matrices as 

shown: 

i = -ioi 

25 j = -icr 2 (Eq. 12) 

k = -i CT3 (Eq. 13) 

(Eq. 13A) 
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where 



10 



15 



a, = 



1 0 

0 1 

0 l" 

1 0 

0 -i 

1 0 
1 0 
0 -1 



(Eq. 14) 
(Eq. 15) 
(Eq. 16) 

(Eq. 17) 
(Eq. 18) 



(See The Theory of Spinors by E. Cartan.) 

Using the definitions of the Pauli spin matrices and the unit quaternion coefficients, the 
unit quaternion can be written as: 

Q = cj 0 cos(9/2) - i(n«o)sin(0/2) (Eq. 19) 

where 

n«a = ni a i + n 2 a 2 + n 3 03 (Eq. 20) 

This can also be shown to be equivalent to the following matrix exponential: 

Q = e ~ 10 ' e/2 where 9 = n 0 (Eq. 2 1 ) 

Note the similarity between this form of the quaternion and the exponential form of a unit 
complex number discussed previously. This form suggests the three-dimensional change in 
"phase" that Hamilton was originally seeking. The appearance of the half angle is accounted for 
20 by the fact that Q is a two-sided transformation. Thus the left and right factors, Q and Q*, each 
contribute half of the desired spatial phase shift. 

It is often more convenient to use the Pauli spin matrix form rather than the traditional 
Hamiltonian form of the quaternion. For example, a vector can be represented as a matrix by 
forming the inner product as shown: 
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X==x.a = x 1 a 1+ x:a 2 + x 3 a3 ^ 22) 

which yields: 

(Eq. 23) 

This matrix form has many useful 
10 properties. ^^^^^^^^^ ^^ 
defined by the uni, „„ mal , a, i s easily produce<1 ^ fc ^ fom] ^ ^ _ ^ 
X' = -AXA where A = a»a 

(Eq.24) 

It can also be shown that any rotation can be produced by two reflect™, If the planes 
n, then the resulting transformation will rotate any vector , x, about the eigenaxis, „, by an angJe, 
X' = BAXAB 

(Eq. 25) 

This two-sided operator, which performs a rotation, closely resembles the quatermon 
rotation described previously. In fact, it can be shown that Q = BA. The following 
multiplicative identity results from the properties of the matrix form of a vector: 
AB = cr 0 a»b + i(axb)«a 

(Eq.26) 

Inthecaseofarotat^^ 

angle 9/2. Therefore, their dot and cross products produce a. b = cos( e/2) and a*b = nsin 9/2) 

wherenisparaJleltothelineofintersection. Substitution of these values into Eq. 26 produces 
the desired result: 

3Q Q = BA = <j 0 cos(9/2) - i(n.a)sin(9/2) = e^* 6 ' 2 (£q 2?) 
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An interesting feature of this development is that the coefficients of the quaternion can be 
found using simple dot and cross products of the appropriate unit vectors. In particular, the two 
vectors, a and b, must be perpendicular to the axis of rotation and separated by an angle of 0/2. 
It can also be shown that given knowledge of two arbitrary vectors a and p in one frame of 
reference and knowledge of their rotated counterparts a' and P' in the rotated frame of reference, 
then the quaternion and the associated rotation matrix responsible for the transformation can be 
uniquely determined. Thus, given two vectors defined in the base frame between the origin and 
two external reference points, the attitude of the system can be uniquely determined by taking 
additional "sightings" of the same two external reference points from the rotated frame and 
comparing the coordinates of the reference vectors measured in the final frame to those measured 
in the initial frame. 

It may also be contemplated to compute quaternion components by direct integration of a 
quaternion rate. This requires an expression for the derivative of the quaternion as a function of 
angular rate. This can be derived by differentiating Eq. 21 . The resulting derivatives of the 
quaternion coefficients are each a linear combination of the angular rate components weighted 
by the existing quaternion components. If the quaternion and angular rates are placed in vector 
form, the following matrix equation results: 



where the dotted q, are the components of the quaternion rate and the coj are the 
components of the angular rate co (=0). 



-?] "?2 "?3 

#3 ?0 "?1 

_"?2 <1\ ?0 



(Eq. 28) 



2 
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referee. For sample i, is ^ „ defme . ^ ^ ^ ^ ^ ^ 



the angular rale is referenced !o the body name. A 



similar matrix equation in which the angular 



rate is referenced to the earth frame is shown below as E q . 29. Note that shifting the reference 
W produces several sign changes in the quaternion matrix. 



10 




(Eq. 29) 



15 



20 



Eq. 29 can be written in a more compact fonn using matrix variables as shown: 
dq/dt = Qcoe/2 

(Eq. 30) 

Eq. 30 can also be solved for a> E as shown below by noting that Q T Q = , : 
«>E=2Q T (dq/dt) 

(Eq.31) 

When the quaternion rate equation is integrated using the instantaneous angular rate of 
the body in question, theresulting quaternion contains the information necessary to transform 
coordmates from the initial frame of reference to the rotated frame. T,us, the quatem^n 
components can be used to fon.^^ g) ^ 

rotation matrix which transforms body coordinates into earth coordinates is shown below in 
terms of quaternion components: 

7(nn „ \ <w_ . -i 

(Eq. 32) 



R EB = 



2(M+«A) ll-ti+ql-ql 2(, 2 ,3- 9l9o) 
2 (ftft-? 2 ?o) 2(q 3 q 2+ g iqo) q l- q l- q l +ql 
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Since the transformation matrix, R E b , is orthogonal, its transpose is the inverse 
transformation, R B e . Therefore, quaternions can be used to track the attitude of a body through 
any angular motion. 

For the purpose of comparison, the equations for computing the Euler angle rates are 

5 shown below. The associated rotation matrix, R, is also shown as a function of the Euler angles. 

dcp/dt = P + (d\|//dt)sin9 
d0/dt = Qcoscp - Rsincp 
d\|//dt = (Qsincp + Rcoscp)sec0 

10 where <p = Roll 

6 = Pitch 

i|/ = Yaw 

cd = Angular Rate Vector = [P Q R] T 



15 R a = 



cos y/ cos 9 cos y/ sin G sin <f> - sin y/ cos <f> cos y/ sin 0 cos </> + sin y/ sin <f 
sin y/cosO sin y/ sin 0 sin ^ + cos y/ cos (p sin y/ sin 6 cos <j> - cos ^ sin </> 
-sin^ cos.^sin^ cos^cos^ 



As is known, the Euler method depends heavily on trigonometric functions and is quite 
computationally intensive. In addition, the Euler rate equations depend on the secant of the pitch 
angle which results in a singularity when the pitch reaches ±90 degrees. In contrast, the 

20 quaternion method contains no trigonometric functions and relies only on multiplication and 
addition. Consequently, the quaternion method is far more computationally efficient. As was 
noted above, the attitude of a body can be uniquely represented in quaternion form, given two 
vectors defined in the body frame and two external reference points. However, it can be difficult 
and/or expensive to establish two external reference points, particularly in the case of terrestrial 

25 objects, whether located on land or on or under the surface of a body of water. 

The present inventor believes he is the first to recognize how quaternion notation may be 
usefully modified such that only one external reference point is needed to generate the modified 
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quaternion form, and that such a modified quaternion fonn may be usefully applied to bodies 
which are free to tilt, but forwhich the heading (yaw)is constrained or need not be controlled by 
the system which employs the modified quaternions. 

OBJECTS AND StJMMAEY QE THE rMVKxrrT^ 
5 It is an object of the invention to provide improved tracking and/or control of tiltable 

objects. 

It is a further object of the invention to provide a tracking and control system which is 
less computationally demanding than prior art systems. 

It is still a further object of the invention to provide quaternion processing to a system in 
0 which only one external reference is available. 

It is yet another object of the invention to provide reliable high-bandwidth tracking of an 
object's orientation. 

An aspect of the invention provides a method of estimating the attitude of a tiltable body, 
where the body includes tl .t sensmg dev.ces such as a fluidic ti ,t sensors and angular rate sensing 
devices such as gyros. The method includes the steps of: outputting angular rate information 
from the angular rate sensing devices, transforming and integrating the outputted angular rate 
information to produce first quaternion position information such that the first quaternion 
position information is constrained to represent a rotation about a horizontal axis in an earth 
frame of reference, outputting tilt information from the tilt sensing devices, processing the 
outputted tilt information to generate second quaternion position information such that the 
second quaternion position information is constrained to represent a rotation about a horizontal 
axis in the earth frame of reference, comparing the first quaternion position information to 
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generate error information, and using the error information to compensate for drift in the angular 
rate sensing devices. 

Another aspect of the invention provides a method of estimating the attitude of a tiltable 
body, where the body includes tilt sensing devices such as fluidic tilt sensors and angular rate 
5 sensing devices such as gyros. The method includes the steps of: outputting angular rate 
information from the angular rate sensing devices, generating modified quaternion rate 
information from the outputted rate information, with the modified quaternion rate information 
being in the form of a scalar quantity and a two-component vector, integrating the modified 
quaternion rate information to generate first modified quaternion attitude information, with the 
10 step of generating the modified quaternion rate information being constrained so that the first 
modified quaternion attitude information is of the form e = eo+eii+e 2 j, where i, j are imaginary 
unit vectors which satisfy the following relations with each other and with a third imaginary unit 
vector k: 

i 2 =j 2 =k 2 =ijk=-l, jk = kjH ki = ik=j; 

15 outputting tilt information from the tilt sensing devices, generating second modified quaternion 
position information from the outputted tilt information, with the second quaternion position 
information being of the form e = eo+eii+e 2 j, comparing the first modified quaternion position 
information to the second modified quaternion position information to generate modified 
quaternion position error information, transforming the modified quaternion position error 

20 information into an angular rate error signal, and using the angular rate error signal to 
compensate for an error in the outputted angular rate information. 

With the method provided in accordance with the invention, tilt information outputted 
from a fluidic sensor or the like is used to compensate for drift or other offsets in a high- 
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bandwidth angular rate sensor, such as a set of gyros. The error compensation provided through 
the tilt reference data allows the corrected high-bandwidth angular rate data to be integrated to 
provide a real-time attitude tracking estimate that does not accumulate offset errors. Heading 
angular rate information (yaw rate information) is discarded to produce a modified (3- 
5 component) quaternion representation that is compared with a similar modified quaternion rate 

information derived from tilt information and based on a single, gravitational reference point. 

Error compensation and filtering are performed with quaternion notation to provide 

computational efficiency. 

The above, and other aspects, features and advantages of the invention will be set forth in 
1 0 and/or apparent from the following description and drawings. 

BRIEF DESC RIPTION OF THF. PR AWTNing 
Fig. 1 is a schematic and generalized side view of a tillable body with respect to which 
the present invention is applied. 

Fig. 2 is a top plan view of the tiltable body of Fig. 1. 
15 Fig. 3 is a block diagram illustration of a control system for the tiltable body of Fig. 1. 

Fig. 4 is a block diagram representation of an attitude and rotation estimation portion, 
provided in accordance with the invention, of the control system of Fig. 3. 

DETAILED DESCRIPTION Q F PRRFFRRFn EMBODTMFNTS 
Fig. 1 is a schematic side view of a generalized tiltable body 10 to which the present 
20 invention is applicable. 

Fig. 2 is a top plan view of the same body 10. The body 10 may be any object for which 
inclination from a level condition is to be tracked, represented and/or controlled. For example, 
the body 10 may be a robot, a tiltable motor-controlled wheelchair, an off-shore drilling 
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platform, a surface ship or boat or submarine water craft, a tiltable railway locomotive or car, a 
passenger-bearing enclosure or vehicle in an amusement ride, a flight simulator, or a vessel used 
to contain and selectively pour out a material used in a chemical reaction or other manufacturing 
process. Body 10 may also be an automobile. Associated with the body 10 is a control system 
5 12 which receives input from one or more tilt sensing devices 14 and one or more angular rate 
sensing devices 16. The tilt sensing devices 14 may be conventional fluidic tilt sensors and/or 
accelerometers. It is contemplated that there be one or more tilt sensors. The angular rate sensor 
16 may be constituted by conventional gyros or other known angular rate sensing devices. It is 
contemplated that there be one or more angular rate sensors. 

10 A coordinate system for the ensuing discussion may be established as illustrated in Figs. 

1 and 2. The horizontal forward direction is considered to be the positive x axis as indicated by 
arrow 20. The positive y axis is considered to be in the horizontal direction pointing to the right 
and perpendicular to the x axis, as indicated by arrow 22 (Fig. 2). The positive z axis is 
considered to be pointing vertically straight down, as indicated by arrow 24 (Fig. 1). Tilt will be 

1 5 defined as an angular deviation from vertical, having a range limited to plus or minus 1 80°. The 
direction of tilt is defined in terms of the x and y axes. Rotation around the x axis will be 
considered to be roll, with a positive roll being tilting to the right. Pitch is defined as rotation 
around the y axis with positive pitch being a backward tilt. Yaw is defined as rotation around the 
z (vertical) axis, with positive yaw being defined as rotation to the right. It will be noted that 

20 these definitions adhere to a right-hand rule. The above definitions of pitch and roll are similar 
to those for Euler pitch and roll, except that Euler angles are applied in a specific order and 
produce intermediate frames of reference. In the tilt-based system discussed herein, it is 
assumed that rotations around the vertical axis are nonexistent or can be disregarded. For 
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example, the heading of the body ,0 may be fixed, or irrelevant to ti.t tracking and control, or 
subject to control by a human operator, or controlled by a system that does not or need not take 
tilt information into account. 

Fig. 3 illustrates, in block diagram form, components of the control system 12 shown in 
5 Fig. 1. The control system 12 includes an interface 40 through which the contro. system 12 
receives signals outputted from the tilt sensors ,4 and gyros 16. These signals are processed by 
an amtude/rotation estimation block 42, in accordance with the invention. On the basis of the 
signais outputted from the gyros and the sensors, block 42 provides an estimate of the attitude 
and/or angular rotation rates of the body 10, in a manner which will be described below. On the 
10 basis of the attitude and/or rotation information provided by block 42, a b.ock 44 generates 
control signals for controlling actuators 46 to control the attitude of the body 10. The actuators 
may include motors, solenoids, floodable chambers, or other conventional mechanical, 
electromechanical, hydraulic or pneumatic devices used to control the attitude of the types of 
bodies referred to above. Electronic hardware for carrying out at least part of blocks 42 and 44 
15 may be constituted by one or more conventional microprocessors. In addition, the control signal 
generation block 44 also preferably includes driving circuitry for providing suitable driving 
signals for the actuators 46. 

Although the control system 12 is indicated in Fig. 1 as being carried on the body 10, it 
should be understood that at least some portions of the contro. system 10 may be physically 
20 separate from the body 10. For example, processing circuitry to carry out some or all of the 
blocks 42 and 44 may be remote from the body 10 and may receive output from the tilt sensors 
14 and gyros 16 via telemetry, and may also transmit control signals back to the body 10, by 
suitable wireless communication channels. 
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Attitude and Rotation Estimation 

Operation of the attitude/rotation estimation block 42 will now be described. 

Tilt sensors, such as conventional fluidic sensors, may be relatively free of drift and 
therefore provide a reliable indication of a gravity vector. Thus the attitude of a static system can 
5 be determined relative to gravity using such sensors. However, in a dynamic system sensors of 
this type are sensitive to angular and vibrational accelerations, and therefore must be low-pass 
filtered to attenuate the effects of accelerations other than gravity. The resulting data provides 
the average direction of gravity over a finite period of time determined by the bandwidth of the 
filter. However, in systems requiring a fast response time, gravitation sensors alone are not 
10 adequate. 

On the other hand, gyros or other angular rate sensors may provide high bandwidth and 
rapid response, without being adversely affected by acceleration. Although the angular rate 
detected by such sensors does not directly indicate the attitude of the system, the rate data can be 
integrated to produce position information. However, rate sensors are subject to drift which can 
15 cause significant errors when integrated. The estimation process provided by block 42 combines 
the high and low bandwidth information respectively provided by the gyros 16 and the tilt 
sensors 14 to provide accurate position and rotation data, using a model-based estimator 
topology. 

In a model-based estimator, an estimate of the true position is produced by comparing 
20 sensor data with an internal model of the system dynamics. The error between the measured data 
and the predicted data is used to continuously refine the estimate. The degree to which this error 
affects the estimate is determined by a weighting matrix, H, that feeds the appropriate state errors 
back to the estimator. The state variable equations for such an estimator are shown below: 
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0 



dxe/dt = Axe + Bu + H(y-y e ) 
y e =Cxe + Du 

dx/dt = Ax + Bu 
y = Cx + Du 

where x = System State Vector 
*e = Estimated State Vector 
y = System Output Vector 
ye Estimated Output Vector 

System/Estimator Input Vector 



u 



> D 



A = System Matrix 
B = Input Matrix 
C = Output Matrix 



Input/Output Matrix 



H - Feedback Matrix 
Note that the subscript «e» denotes estimated parameters. The internal system model is 
defined by matrices (A,B,C,D). The feedback matrix H determines the stability of the estimator. 
A stable estimator will, over time, reach an equilibrium where the actual and estimated states are 
equal - x). The internal model is often based on simple integrators. The effectiveness of 
such a simple model is related to the fact that integration and differentiation are fundamental to 
Physical phenomena. For example, rate is the derivative of position: v = dx/dt. Therefore, given 
a simple, first-order integrator with parameters A=0, B=l, 0-1, D=0, and a feedback gam H=k, , 
it is possible to produce an estimator which combines the measured static position of the system, 
Go , and the measured dynamic position, 9, such that the estimate, 9e, is accurate over the full 
frequency range. In effect, the estimate is a frequency-dependent weighted sum of the two 
measurements. The Laplace transform of this result is shown below: 

9e(s) = 6 0 (s)[ k,/(s+k,)] + 9(s)[s/(s+k,)] (£q 33) 

Note that at low frequency ( s = 0 ), the estimated position approaches the low bandwidth 
position measurement, 9 0 , and at high frequency, the estimate posxtion approaches the high 
bandwidth position measurement, 9. Thus, the high and low bandwidth data are combined. 
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A more useful estimate can be obtained if the estimator is based on a simple, second- 
order integrator. In this case, a feedback matrix of the form H=[ki k 2 ] T can be used to feedback 
the low bandwidth measurement, 9o , via the output matrix C=[l 0]. The Laplace transform of 
the resulting position and rate estimates are shown below: 

0e(s) = co(s)[s/(s 2 +k,s+k 2 )] + 9o(s)[(k,s+k 2 )/(s 2 +k lS +k 2 )] (Eq. 34) 

oe(s) = co(s)[(s 2 +k,s)/(s 2 +k,s+k 2 )] + 9 0 (s)[k 2 s/(s 2 +k,s+k 2 )] (Eq. 35) 

where (o(s) is the measured value of d8/dt in the s-domain. Therefore, this estimator topology 
combines the low bandwidth position, 9o , with the high bandwidth rate, co. At low frequency, 
the estimated position approaches the low bandwidth measurement, 9 0 , as before. At high 
frequency, the estimated position approaches the integral of the high bandwidth rate 
measurement, ©, which is equivalent to the high bandwidth position, 8. A similar result is 
obtained for the estimated rate. Thus, this type of estimator is well suited for producing an 
estimate of angular position and rate from low bandwidth tilt data and high bandwidth angular 
rate data. The gains k\ and k 2 are chosen such that the appropriate filter bandwidth and stability 
are obtained. Note that letting k 2 =9 and co(s)=s9(s) in Eq. 34 produces the same first-order 
position estimate shown in Eq. 33. 

It is known that quaternions can be used to track the attitude of a system through any 
angular motion. Moreover, quaternions are a computationally efficient notation and also are 
immune to the singularities encountered by Euler angles. It is also known that data from high 
and low bandwidth sensors can be combined using an estimator to provide accurate data over the 
full bandwidth. However, quaternions do not lend themselves to direct physical measurement, 
and this complicates the derivation of an absolute quaternion reference from tilt sensor data. 
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Without such a reference, it is impossible for the estimator to prevent gyro drift from corrupting 
the integrated quaternion and eventually causing the system to lose track of its angular position. 

From previous discussion it will be recalled that conventional quaternions require two 
rotated vectors and two external references to uniquely represent a rotation. However, with tilt 
sensors alone only a single reference, to the center of the earth, is available. The present 
invention solves this problem for tilt-based systems by providing a modified quaternion notation 
in which only three elements of the four conventional elements are non-zero, and only a single 
reference is needed to uniquely represent attitude (tilt) in quaternion form. This solution 
assumes that the system does not require knowledge of its heading and therefore does not need to 
track changes in yaw (rotation about the vertical axis). 

Based on the previous development, computing the quaternion coefficients from a single 
reference point requires that the reference vector in each frame be perpendicular to the axis of 
rotation. For a tilt-based system with gravity as its reference, this is equivalent to requiring that 
the axis of rotation remain horizontal at all times. If the axis of rotation, n, is always horizontal, 
then the z-component of the axis must be identically zero. Referring to the definition of the unit 
quaternion components, this implies that the q 3 component of the quaternion must also be 
identically zero. Therefore, the condition q 3 = 0 becomes a constraint. The resulting quaternion 
has thus been modified and contains only three significant components. 

The constraint q 3 =0 must also be satisfied while the quaternion rate equation (Eq. 28 or 
29) is being integrated. This condition can be maintained during the integration as long as the 
condition dq 3 /dt = 0 is also maintained. From Eqs. 28 and 29, this implies that one of the 
following conditions must hold: 

0 = dq 3 /dt = -qjffl, + q,o) 2 + qom (For integration in the Body Frame) (Eq. 36) 
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0 = dq 3 /dt = q 2 a>Ei - qi«E2 + qo©E3 (For Integration in the Earth Fame) (Eq. 37) 
In order to satisfy one of these constraints, at least one parameter must be allowed to vary 
arbitrarily as required. In the body frame, the three angular rate components and the three 
modified quaternion components all contain necessary information. However, in the earth 
frame, it has been assumed that the yaw component of the angular rate is not important because 
the system is a tilt-based. Therefore, the true yaw component can be replaced with a virtual yaw 
component that satisfies the above constraint. In practice, this may be done by transforming the 
measured angular rate from the body frame into the earth frame. The resulting earth yaw can 
then be replaced with a virtual earth yaw which satisfies the following constraint equation: 
co'e3 = (qi©E2 - q2©£i )/ q 0 (Virtual Earth Yaw Satisfying dq 3 /dt =0) (Eq. 38) 
This earth yaw component can be substituted back into the original quaternion rate 
equation (Eq. 29) to eliminate its explicit dependence on yaw. In order to distinguish the 
modified three-element quaternion from other quaternions, the modified quaternion will now be 
denoted by "e" rather than "q" The components can then be written: 

eo = cos(0 12) and e = [e, e 2 ] T = nsin(0/2) (Eq. 39) 

where n = [n x n 2 ] T and e 3 = n 3 = 0 

After eliminating co E 3 and e 3 , the modified quaternion rate equation becomes: 

where I = 2x2 Identity (Eq. 40) 

The rotation matrix R EB (Eq. 32) can also be simplified by eliminating a) E3 and e 3 as 

shown: 

Reb A 1 ' 2 ff T 2 f e o 1 where f = [e 2 - ei ] T (Eq. 41) 
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Combining these results and noting that a> E = R eb£0) the modified quaternion rate 
referenced to the earth frame can be written as a function of the angular , 
shown: 



" rate in the body frame as 



• 
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2e 0 _ 



-e 0 e' 



lle--ff r 2e 0 f 



CO, 



a>. 



■3 j 



(Eq. 42) 



This can be stated more succinctly in matrix form as shown: 
de/dt = Eco/2 

10 (Eq. 43) 

Usmg this result, the modified quaternion rates can be integrated to produce the modified 
quaternion. However, this equation assumes an ideal angular rate vector, » , and provides no 
compensation for the drift in the actual angular rate data from the gyros. In genera., an offset 
must be subtracted from the raw gyro data to correct for the drift. The corrected angular rate of 

15 the form ©' = © — co drift is then used in place of a, in the above equation. The situation is further 
complicated by the fact that the gyro drift is a function of time. Therefore, the drift correction 
term must be continuously updated. This requues the use of the second order estimator topology 
described earlier. 

The purpose of the estimator is to compare the quaternion obtained by integration of the 
20 quaternion rate to the quaternion obtained from the tilt data. Assuming that any steady-state 
error is due to gyro drift only, the magnitude of this error can be used to adjust the gyro drift 
correction term. The estimator gains are chosen such that the steady-state error is, over time, 
driven to zero. Selection of estimator gain values is well within the abilities of those who are 
skilled in the art, and therefore need not be discussed further. 
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In order to properly adjust the correction term, the quaternion error must be converted 
back to an angular rate error. This is accomplished using the original quaternion rate equation 
which has been solved for angular rate in the earth frame as shown: 

co E = 2Q T (dq/dt) (Eq. 3 1 , repeated) 

5 

Integrating this expression with respect to time produces the following result: 

AG e = 2Q T (Aq) = 2Q T (q> - q) (Eq. 44) 

10 Therefore, the above equation can be used to convert the error between the estimated 

quaternion and the tilt quaternion (Sq = q' - q) into a tilt angle error, A0 E . It is recalled that in 
this case we are using the modified quaternion for which q3 = 0 in both q and Q. However, the 
earth yaw component obtained in this way is not valid because the quaternion was generated 
using the virtual earth yaw. In order to produce a valid angular error, the original earth yaw must 
15 be substituted in place of the error component A6 E 3. The expression for the earth yaw can be 
derived from ©e = Red© with q3 = 0 and is given below: 

co E3 = (-2q 2 q 0 )coi + (2q,q 0 )(O2 + (q 0 2 -qi 2 -q2 2 )«3 (Eq. 45) 

After substitution, the resulting angular error vector in the earth frame is given by AO E ' = 
20 [A9 E i A9e2 03 E 3] T . Since the gyros are in the body frame, the angular error in the earth frame 
must be transformed into the body frame using AG = RbeAQe'. The resulting angular error can 
then be used by the estimator to produce the gyro drift correction term. 

The method by which the tilt data is converted to a modified quaternion will now be 
discussed. It has been shown that the quaternion components can be produced by forming the 
25 dot and cross products of two unit vectors which are perpendicular to the axis of rotation and 
separated by an angle of 6/2. Two such vectors can be constructed using the external reference 
point at the center of the earth (i.e. the direction of gravity as measured by the tilt sensor). Let 
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the Mai ^av,,y vector be denoted as G, »d ,« fte final ^ vector be ^ by G< (ne 

and smmd for ftrto us. The "final" gmvi* veeror G,is .o be opdared as rhe esfimaror 
operates.) 

5 Let these two vectors be normalized as shown: 

8 '" G '" G ' 1 ™> *=GflGt (E,.46) 

.0 follows"*" 6 ** n0n " aliZe<1 V «°* - **«• unit vectors can be constnrcted as 

a-( S + g ,M S+gi an d „ = 6 (£q47) 

Finally, ft. ona.em.on component, can be genera.ed by compodng ,he do. and cmss 
1 5 products as shown below: 

^ e = axb (Eq.48) 

Byconstn.ction.thecrossproductofaa.dbproducesavectorw^^ 
the gravity vectorin each ^ Co-e^.^^^^,^ 

three events of the ouatemion are non-trivial and satisfy the form of the modified ouaternion. 
The overall estimation process described above is illustrated in the block diagram of Fig 

4. 

The eanmaror topology is split into rwo loops me k, loop and the kj ,„„ p . The t, loop 
25 provides low bandwidth filtering of thedata from ,he ,i„ !aBor , mi detemiinK fc ^ of 
-or ma, i5 nsed ,o adjns, the esrimated ooatemion ra... Tbe b .oop provides drifi conocrion for 
me gyro data. Since ,he gyaos arc in me body frame, me fa loop m„ s , , ie on <he My ^ 
coordinate ,ran S f„rma,ion. The K, gain detem]lnes fc ^ „, ^ ^ . ^ ^ ^ ^ 
dnft in me gyro da* The weighted e^r is .hen in.egra.ed so that mo loop wiU, over „me, 
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achieve zero steady-state error. In other words, when the quaternion error reaches zero, the 
output of the k 2 integrator stops changing and the drift correction term remains constant. 

In Fig. 4, a summation block 50 in the k 2 loop applies a weighted error signal (which may 
also be considered a drift correction signal) to the angular rate information outputted from the 
gyros 16, to produce corrected angular rate information. The corrected rate information is 
converted to the earth frame of reference and transformed into a modified quaternion at block 52. 
As indicated by the label E/2 in block 52, the calculations applied in that block are in accordance 
with equations 43 and 42, which were discussed above. The resulting estimated rate 
information, which is in the above-described modified quaternion form is provided as an input to 
a summation block 54. At block 54, a correction provided by the k] loop is applied to the 
estimated quaternion rate information to produce data indicative of the change in the estimated 
position. This data (which may be referred to as an estimated position difference signal) is, in 
turn, integrated at block 56 to provide the modified quaternion estimated position data. At block 
58, the modified-quaternion estimated position data outputted from block 56, and also the 
estimated modified-quaternion rate information outputted from block 52, are converted to Euler 
angles to be outputted to the control signal generation block 44 (Fig. 3). This conversion is 
easily accomplished using the following equations. (Although these equations use trigonometric 
functions, the computational complexity is minimal and can easily be performed using lookup 
tables.) 

9 = asin(2e 2 e 0 ) and <p = asin(2eieo/cos6) (Eq. 49) 

These equations can also be simplified using small-angle approximations in the event that 
the range of motion of the tilt-based system is limited. 
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If the control signal generator 44 is of the type which outputs quaternion signals rather 
than Euler signals, than block 58 may be omitted and the estimated quaternion position and rate 
information (in the modified form provided in accordance with the invention) may be provided 
directly to the control signal generator. 

In any case, the estimated position information in modified quaternion form is provided 
as an input to a summation block 60, at which it is compared to the current tilt information 
outputted from the tilt sensors 1 4, as converted to the modified quaternion form at block 62. 
(The signal produced at block 62 may be referred to as a modified quaternion reference position 
signal.) The conversion carried out at block 62 is in accordance with the procedure described 
above in connection with Eqs. 46-48. The resulting error signal outputted from summation block 
60 is weighted by the gain factor k, at block 64, and then is applied as the above-mentioned 
correction signal at block 54. The output from block 60 (which may be considered an error 
position signal) is also provided as an input to block 66. In accordance with Eq. 44, block 66 
converts the error between the estimated quaternion and the tilt quaternion into a tilt angle error. 
The invalid yaw component of the resulting signal output from block 66 is replaced with the 
earth-frame yaw component which is available from the processing of block 52, and the resulting 
angular error vector is converted to the body frame of reference at block 68. The output of block 
68 is then integrated and weighted by the k 2 gain at block 70, and the resulting signal is applied 
at summation block 50 as the above-mentioned drift correction signal. 

It should be noted that the coefficients required for the processes in blocks 52, 66 and 68 
are taken from the modified-quatemion position information outputted from the integration block 
56, as is indicated at 72 in Fig. 4. 
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As will be appreciated by those who are skilled in the art, the processes illustrated in 
block diagram form in Fig. 4 may advantageously be performed by one or more suitably 
programmed computing devices (e.g. one or more microprocessors) which constitute part of the 
control system 12. 

In the attitude and rotation estimator provided in accordance with the invention, high 
bandwidth information provided by rotation sensors and low bandwidth information from tilt 
sensors are combined in a novel modified quaternion notation having three elements instead of 
the conventional four elements. The resulting estimated quaternion is valid over the designed 
bandwidth and does not drift over time. The computations to be performed can be handled 
efficiently using simple arithmetic such as multiplication, addition and square root. The 
modified quaternion estimator is applicable to a broad range of systems in which tilting is to be 
tracked and/or controlled. 

The above description of the invention is intended to be illustrative and not limiting. 
Various changes or modifications in the embodiments described may occur to those skilled in the 
art and these can be made without departing from the spirit or scope of the invention. 
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What is claimed is : 

1 . A method of estimating the attitude of a tiltable body, said body including tilt 
sensing means and angular rate sensing means, the method comprising the steps of: 

outputting angular rate information from said angular rate sensing means; 

transforming and integrating said outputted angular rate information to produce first 
quaternion position information such that said first quaternion position information is 
constrained to represent a rotation about a horizontal axis in an earth frame of reference; 

outputting tilt information from said tilt sensing means; 

processing said outputted tilt information to generate second quaternion position 
information such that said second quaternion position information is constrained to represent a 
rotation about a horizontal axis in said earth frame of reference; 

comparing said first quaternion position information to said second quaternion position 
information to generate error information; and 

using said error information to compensate for drift in said angular rate sensing means. 
2. A method of estimating the attitude of a tiltable body, said body including tilt 
sensing means and angular rate sensing means, the method comprising the steps of: 
outputting angular rate information from said angular rate sensing means; 
generating modified quaternion rate information from said outputted angular rate 
information, said modified quaternion rate information being in the form of a scalar quantity and 
a two-component vector; 

integrating said modified quaternion rate information to generate first modified 
quaternion attitude information, said step of generating said modified quaternion rate information 
being constrained so that said first modified quaternion attitude information is of the form 
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e=e 0 +eii+e 2 j, where i, j are imaginary unit vectors which satisfy the following relations with 
each other and with a third imaginary unit vector k: 

i 2 = j 2 = k 2 =ijk = -1, ij = -ji = k, jk = -kj = i, ki = -ik = j; 
outputting tilt information from said tilt sensing means; 

generating second modified quaternion position information from said outputted tilt 
information, said second modified quaternion position information being of said form e = e 0 + en 

comparing said first modified quaternion position information to said second modified 
quaternion position information to generate modified quaternion position error information; 

transforming said modified quaternion position error information into an angular rate 
error signal; and 

using said angular rate error signal to compensate for an error in said outputted angular 
rate information. 

3. A method according to claim 2, further comprising the step of low pass filtering 
said second modified quaternion position information. 

4. A method according to claim 2, wherein: 

said angular rate information is provided in a first frame of reference and said tilt 
information is generated in a second frame of reference different from said first frame of 
reference; 

said step of generating said modified quaternion rate information includes converting said 
outputted angular rate information from said first frame of reference to said second frame of 
reference; and 
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further comprising the step of converting said angular rate error signal from said second 
frame of reference to said first frame of reference before said using step. 

5. A method according to claim 2, wherein said second modified quaternion position 
information is generated on the basis of first tilt information outputted from said tilt sensing 
means and stored on a first occasion and second tilt information outputted from said tilt sensing 
means on a second occasion different from said first occasion. 

6. A method of estimating the attitude of a tiltable body, comprising the steps of: 
generating angular rate information in a body frame; 

applying a drift correction signal to said angular rate information to produce corrected 
angular rate information; 

processing said corrected angular rate information using modified quaternion estimated 
position information to produce modified quaternion estimated rate information referenced to an 
earth frame, said modified quaternion estimated rate information consisting of a scalar 
component and two vector components; 

adjusting said modified quaternion estimated rate information on the basis of a weighted 
position error signal to produce an estimated position difference signal; 

integrating said estimated position difference signal to produce said modified quaternion 
estimated position information, said modified quaternion estimated position information 
consisting of a scalar component and two vector components; 

generating tilt information in said earth frame; 

using said tilt information to generate modified quaternion reference position 
information, said modified quaternion reference position information consisting of a scalar 
component and two vector components; 
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subtracting said modified quaternion reference position information from said modified 
quaternion estimated position information to produce an error position signal; 

weighting said error position signal to produce said weighted position error signal; 

converting said error position signal to a tilt angle error signal using said modified 
quaternion estimated position information; 

forming an angular error vector from pitch and roll components of said tilt angle signal 
and from a yaw component of an angular rate signal generated by converting said corrected 
angular rate information to said earth frame using said modified quaternion estimated position 
information; 

converting said angular error vector from said earth frame to said body frame using said 
modified quaternion estimated position information; and 

applying weighted integration to said converted angular error vector to generate said drift 
correction signal. 

7. A method according to claim 6, further comprising the steps of: 
converting said modified quaternion estimated rate information and said modified 

quaternion estimated position information to generate Euler angle output information; and 
outputting said Euler angle output information to control signal generating means. 

8. Apparatus for estimating the attitude of a tiltable body, comprising: 
tilt sensing means mounted on said body for outputting tilt information; 

angular rate sensing means mounted on said body for outputting angular rate information; 

means for generating modified quaternion rate information from said angular rate 
information outputted from said angular rate sensing means, said modified quaternion rate 
information being in the form of a scalar quantity and a two-component vector; 
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means for integrating said modified quaternion rate information to generate first modified 
quaternion attitude information, said first modified quaternion attitude information being of the 
form e = e 0 + eji + ej; where i, j are imaginary unit vectors which satisfy the following relations 
with each other and with a third imaginary unit vector k: 

i 2 =j 2 = k 2 = ijk = -U ij = -ji = k, jk = -kj = i, ki = -ik = j; 

means for generating second modified quaternion position information from said tilt 
information outputted from said tilt sensing means, said second modified quaternion position 
information being of said form e = e 0 + eii + e2j; 

means for comparing said first modified quaternion position information to said second 
modified quaternion position information to generate modified quaternion position error 
information; 

means for transforming said modified quaternion position error information into an 
angular rate error signal; and 

means for compensating for an error in said angular rate information outputted from said 
angular rate sensing means on the basis of said angular rate error signal. 

9. Apparatus according to claim 8, wherein said tilt sensing means includes at least 
one fluidic tilt sensor. 

10. Apparatus according to claim 9, wherein said angular rate sensing means includes 
at least one gyro. 

1 1 . Apparatus according to claim 8, further comprising means for converting said 
modified quaternion rate information into Euler angle rate information and for converting said 
first modified quaternion attitude information into Euler angle position information. 



-29- 



WO 01/57474 



PCT/US01/03290 



1/2 



FIG. 1 



BODY' 



roll 



20 

L 



10 



TILT 
SENSORS 



14 



GYROS 



16 



CONTROL 
SYSTEM 



12 



yaw 

FRONT 

x(+) 



24 



z(+) 



FIG. 2 



20 

roll ) 

-3 — L 




pitch 
-x(+) 



FRONT 



-22 



y(+) 



SUBSTITUTE SHEET (RULE 26) 



WO 01/57474 



PCT/US01/03290 



2/2 





SUBSTITUTE SHEET (RULE 26) 



INTERNATIONAL SEARCH REPORT 



Internet! Application No 

PCT/US 01/03290 



A. CLASSIFICATION OF SUBJECT MATTER 

IPC 7 G01C21/16 G05D1/08 



According to International Patent Classification (IPC) or to both national classification and IPC 



a FIELDS SEARCHED 



Minimum documentation searched (classification system followed by classification symbols) 

IPC 7 G01C G05D 



Documentation searched other than minimum documentation to the extent that such documents are included in the fields searched 



Electronic data base consulted during the international search (name of data base and, where practical, search terms used) 

WPI Data, PAJ, EPO-Internal , INSPEC, IBM-TDB, COHPENDEX 



C. DOCUMENTS CONSIDERED TO BE RELEVANT 



Category ° Citation of document, with indication, where appropriate, of the relevant passages 



Relevant lo claim No. 



US 5 875 993 A (WEISS HAIM ET AL) 
2 March 1999 (1999-03-02) 
cited In the application 
the whole document 



1,2,6,8 



□ 



Further documents are listed in the continuation of box C. 



ID 



Patent family members are listed in annex 



• Special categories of cited documents : 

"A" document defining the general state of the art which is not 

considered to be of particular relevance 
"E" earlier document but published on or after the international 

filing date 

'L' document which may throw doubts on priority daim(s) or 
which is cited to establish the publication date of another 
citation or other special reason (as specified) 

'O* document referring to an oral disclosure, use. exhibition or 
other means 

•p« document published prior to the international filing date but 
later than the priority date claimed 



"T later document published after the international filing date 
or priority date and not in conflict with the application but 
cited to understand the principle or theory underlying the 
invention 

'X' document of particular relevance; the claimed invention 
cannot be considered novel or cannot be considered to 
involve an inventive step when the document is taken alone 

'V document ol particular relevance; the claimed invention 
cannot be considered to involve an inventive step when the 
document is combined with one or more other such docu- 
ments, such combination being obvious to a person skilled 
in the art. 

document member of the same patent family 



Date of the actual completion of the international search 



18 May 2001 



Date of mailing of the international search report 



01/06/2001 



Name and mailing address of the ISA 

European Patent Office, P.B. 5818 Patentban 2 
NL - 2280 HV Rijswijk 
TeL (+31-70) 340-2040, Tx. 31 651 epo nl, 
Fax: (+31-70) 340-3016 



Authorized officer 



Hoekstra, F 



Form PCT71SA/2 10 (second sheet) (July 1992) 



INTERNATIONAL SEARCH REPORT 

InTm maUon on patent family members 



Internal) Application No 

PCT/US 01/03290 



Patent document 
cited in search report 



Publication 
date 



Patent family 
member(s) 



Publication 
date 



US 5875993 



02-03-1999 



IL 



118883 A 



01-06-2000 



Form PCT/1SA/210 (patent family annex) (July 1992) 



